const int SonicSensorPin = 8;

short get_range_sensor_sonic(const int iSonicPin, const int iCount=5, const int iDelay=0, const float fBeamPrecision=0, const float fMin=0, const float fMax=0)
{
  float fValMin = -1, fValMax = -1;
  float fDistance = 0;
  float fCount = 0;
  boolean bLimitRange = (fMax>fMin)?true:false;
  boolean bUsePrecision = (fBeamPrecision>0)?true:false;

  for(int i=0; i<iCount; i++)
  {
    if(iDelay>0) delay(iDelay);

    pinMode(iSonicPin, OUTPUT);
    digitalWrite(iSonicPin, LOW);
    delayMicroseconds(2);
    digitalWrite(iSonicPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(iSonicPin, LOW);

    pinMode(iSonicPin, INPUT);
    long duration = pulseIn(iSonicPin, HIGH);

    float fVal = (float)duration/57.0f;
    if(!bLimitRange || (bLimitRange && fVal>=fMin && fVal<=fMax))
    {
      if(bUsePrecision)
      {
        if(fValMax==-1 || fVal>fValMax) fValMax = fVal;      
        else if(fValMin==-1 || fVal<fValMin) fValMin = fVal;
      }
      fDistance += fVal;
      fCount ++;
    }
  }

  // Sum result
  if(fCount>1)
  {
    fDistance /= fCount;

    // ecart trop important = bad result
    if(bUsePrecision && abs(fValMax)-abs(fValMin)>((fMax-fMin)*fBeamPrecision)) return bLimitRange?fMax:-1;
  }

  if(bLimitRange)
  {
    // out of range ?
    if(fDistance>fMax) fDistance = fMax;
    else if(fDistance<fMin) fDistance = fMin;
  }

  // retour en cm
  return (short)fDistance;
}

void setup()
{
  Serial.begin(9600);
  Serial.println("Start!");  
  pinMode(SonicSensorPin,INPUT);
}

void loop()
{
  Serial.print("Dist :");
  //int distance = get_range_sensor_sonic(SonicSensorPin);
  //int distance = get_range_sensor_sonic(SonicSensorPin, 10, 5);
  //int distance = get_range_sensor_sonic(SonicSensorPin, 10, 10, 0.15, 0, 0);
  //int distance = get_range_sensor_sonic(SonicSensorPin, 5, 5, 0.15, 6, 600);
  int distance = get_range_sensor_sonic(SonicSensorPin, 10, 7, 0.10, 7, 550);
  Serial.print(distance, DEC);
  Serial.println(" cm");  
  delay(250);
}
